/** @file r_frustum.cpp
    @brief Camera viewing frustum */

#include "render/r_frustum.h"

using namespace G3;

//==============================================================================
// Constructor
//==============================================================================
G3::FRUSTUM::FRUSTUM () {
    FieldOfView = 45.0f;
    AspectRatio = 4.0f / 3.0f;
    NearPlane = 0.01f;
    FarPlane = 100.0f;
}

//==============================================================================
// = operator
//==============================================================================
FRUSTUM& G3::FRUSTUM::operator= (const FRUSTUM &f) {
    memcpy (Plane, f.Plane, 6 * sizeof (PLANE3F));
    memcpy (Corner, f.Corner, 8 * sizeof (VEC3F));
    memcpy (Intersections, f.Intersections, 12 * sizeof (INTERSECTION3F));

    FieldOfView = f.FieldOfView;
    AspectRatio = f.AspectRatio;
    NearPlane = f.NearPlane;
    FarPlane = f.FarPlane;

    Origin = f.Origin;

    UpVector = f.UpVector;
    RightVector = f.RightVector;
    ForwardVector = f.ForwardVector;

    return *this;
}

//==============================================================================
// Extracts the View Frustum from OpenGL Projection and Modelview matrices
//==============================================================================
bool G3::FRUSTUM::glExtract () {
    MATRIX4x4F proj, modl, clip;

    //This extracts projection and modelview matrices:
    glGetFloatv (GL_PROJECTION_MATRIX, proj.m);
    glGetFloatv (GL_MODELVIEW_MATRIX, modl.m);

    clip = modl * proj;

    Extract (&clip);
    return true;
}

//==============================================================================
// Extracts the View Frustum from a matrix
//==============================================================================
bool G3::FRUSTUM::Extract (float M [16]) {
    //Left plane:
    Plane [GF_LEFT].n.x = M [3] + M [0];
    Plane [GF_LEFT].n.y = M [7] + M [4];
    Plane [GF_LEFT].n.z = M [11] + M [8];
    Plane [GF_LEFT].d = M [15] + M [12];
    Plane [GF_LEFT].Normalize ();

    //Right plane:
    Plane [GF_RIGHT].n.x = M [3] - M [0];
    Plane [GF_RIGHT].n.y = M [7] - M [4];
    Plane [GF_RIGHT].n.z = M [11] - M [8];
    Plane [GF_RIGHT].d = M [15] - M [12];
    Plane [GF_RIGHT].Normalize ();

    //Top plane:
    Plane [GF_TOP].n.x = M [3] - M [1];
    Plane [GF_TOP].n.y = M [7] - M [5];
    Plane [GF_TOP].n.z = M [11] - M [9];
    Plane [GF_TOP].d = M [15] - M [13];
    Plane [GF_TOP].Normalize ();

    //Bottom plane:
    Plane [GF_BOTTOM].n.x = M [3] + M [1];
    Plane [GF_BOTTOM].n.y = M [7] + M [5];
    Plane [GF_BOTTOM].n.z = M [11] + M [9];
    Plane [GF_BOTTOM].d = M [15] + M [13];
    Plane [GF_BOTTOM].Normalize ();

    //Near plane:
    Plane [GF_NEAR].n.x = M [3] + M [2];
    Plane [GF_NEAR].n.y = M [7] + M [6];
    Plane [GF_NEAR].n.z = M [11] + M [10];
    Plane [GF_NEAR].d = M [15] + M [14];
    Plane [GF_NEAR].Normalize ();

    //and Far plane:
    Plane [GF_FAR].n.x = M [3] - M [2];
    Plane [GF_FAR].n.y = M [7] - M [6];
    Plane [GF_FAR].n.z = M [11] - M [10];
    Plane [GF_FAR].d = M [15] - M [14];
    Plane [GF_FAR].Normalize ();
    return true;
}

bool G3::FRUSTUM::Extract (MATRIX4x4F *M) {
    //Left plane:
    Plane [GF_LEFT].n.x = M->m [3] + M->m [0];
    Plane [GF_LEFT].n.y = M->m [7] + M->m [4];
    Plane [GF_LEFT].n.z = M->m [11] + M->m [8];
    Plane [GF_LEFT].d   = M->m [15] + M->m [12];
    Plane [GF_LEFT].Normalize ();

    //Right plane:
    Plane [GF_RIGHT].n.x = M->m [3] - M->m [0];
    Plane [GF_RIGHT].n.y = M->m [7] - M->m [4];
    Plane [GF_RIGHT].n.z = M->m [11] - M->m [8];
    Plane [GF_RIGHT].d   = M->m [15] - M->m [12];
    Plane [GF_RIGHT].Normalize ();

    //Top plane:
    Plane [GF_TOP].n.x = M->m [3] - M->m [1];
    Plane [GF_TOP].n.y = M->m [7] - M->m [5];
    Plane [GF_TOP].n.z = M->m [11] - M->m [9];
    Plane [GF_TOP].d   = M->m [15] - M->m [13];
    Plane [GF_TOP].Normalize ();

    //Bottom plane:
    Plane [GF_BOTTOM].n.x = M->m [3] + M->m [1];
    Plane [GF_BOTTOM].n.y = M->m [7] + M->m [5];
    Plane [GF_BOTTOM].n.z = M->m [11] + M->m [9];
    Plane [GF_BOTTOM].d   = M->m [15] + M->m [13];
    Plane [GF_BOTTOM].Normalize ();

    //Near plane:
    Plane [GF_NEAR].n.x = M->m [3] + M->m [2];
    Plane [GF_NEAR].n.y = M->m [7] + M->m [6];
    Plane [GF_NEAR].n.z = M->m [11] + M->m [10];
    Plane [GF_NEAR].d   = M->m [15] + M->m [14];
    Plane [GF_NEAR].Normalize ();

    //and Far plane:
    Plane [GF_FAR].n.x = M->m [3] - M->m [2];
    Plane [GF_FAR].n.y = M->m [7] - M->m [6];
    Plane [GF_FAR].n.z = M->m [11] - M->m [10];
    Plane [GF_FAR].d   = M->m [15] - M->m [14];
    Plane [GF_FAR].Normalize ();
    return true;
}

//==============================================================================
// Is the point in the frustum?
//==============================================================================
bool G3::FRUSTUM::IsPointIn (VEC3F P) {
    for (uint i=0; i<6; i++)
        if (Plane [i].GetDistance (P) < 0)
            return false;

    return true;
}

//==============================================================================
// Is the sphere in the frustum, out of it, or spanning?
//==============================================================================
FRUSTUM_CLASSIFIC G3::FRUSTUM::IsSphereIn (const BSPH3F &sphere) const {
    for (uint i=0; i<6; i++) {
        if (Plane [i].GetDistance (sphere.Position) < -sphere.Radius)
            return GFC_OUT;
        if (fabs (Plane [i].GetDistance (sphere.Position)) < sphere.Radius)
            return GFC_HALF;
    }

    return GFC_IN;
}

//==============================================================================
// Is the box in the frustum, out of it, or spanning?
//==============================================================================
FRUSTUM_CLASSIFIC G3::FRUSTUM::IsBoxIn (const AABB3F &box) const {
    uint inside = 0;
    bool notout = false;

    // A fast "out" check
    if (BBox.Minimum.x > box.Maximum.x || BBox.Maximum.x < box.Minimum.x ||
        BBox.Minimum.y > box.Maximum.y || BBox.Maximum.y < box.Minimum.y ||
        BBox.Minimum.z > box.Maximum.z || BBox.Maximum.z < box.Minimum.z)
        return GFC_OUT;

    for (uint i=0; i<6; i++) {
        notout = false;
        if (Plane [i].GetDistance (box.Minimum) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (box.Maximum) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Minimum.x, box.Maximum.y, box.Minimum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Maximum.x, box.Minimum.y, box.Maximum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Minimum.x, box.Maximum.y, box.Maximum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Minimum.x, box.Minimum.y, box.Maximum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Maximum.x, box.Maximum.y, box.Minimum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (Plane [i].GetDistance (VEC3F (box.Maximum.x, box.Minimum.y, box.Minimum.z)) >= 0) {
            inside++;
            notout = true;
        }
        if (notout)
            continue;
        else
            return GFC_OUT;
    }
    // All of the points are inside?
    if (inside == 48)
        return GFC_IN;
    // Some of the points are not?
    return GFC_HALF;
}

//==============================================================================
// Sets aspect ratio according to the given window dimensions
//==============================================================================
void G3::FRUSTUM::SetAspectRatio (uint Width, uint Height) {
    float w = Width, h = Height;
    AspectRatio = w / h;
}

//==============================================================================
// Sets the field of view angle (in degrees)
//==============================================================================
void G3::FRUSTUM::SetFieldOfView (float FOV) {
    FieldOfView = FOV;
}

//==============================================================================
// Sets the Near and Far plane distances from the frustum origin
//==============================================================================
void G3::FRUSTUM::SetNearFarDistance (float NearDist, float FarDist) {
    NearPlane = NearDist;
    FarPlane = FarDist;
}

//==============================================================================
// Updates the frustum orientation
//==============================================================================
void G3::FRUSTUM::UpdateOrientation (VEC3F Pos, VEC3F Up, VEC3F Forward, VEC3F Right) {
    Origin = Pos;
    UpVector = Up;
    ForwardVector = Forward;
    RightVector = Right;

    CalculateCorners ();
}

//==============================================================================
// Gets the center of the Near plane
//==============================================================================
VEC3F G3::FRUSTUM::GetNearCenter () {
    return Origin + ForwardVector * NearPlane;
}

//==============================================================================
// Gets the center of the Far plane
//==============================================================================
VEC3F G3::FRUSTUM::GetFarCenter () {
    return Origin + ForwardVector * FarPlane;
}

//==============================================================================
// Gets the height of the Near plane
//==============================================================================
float G3::FRUSTUM::GetNearPlaneHeight () {
    return 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * NearPlane;
}

//==============================================================================
// Gets the width of the Near plane
//==============================================================================
float G3::FRUSTUM::GetNearPlaneWidth () {
    return AspectRatio * 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * NearPlane;
}

//==============================================================================
// Gets the height of the Far plane
//==============================================================================
float G3::FRUSTUM::GetFarPlaneHeight () {
    return 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * FarPlane;
}

//==============================================================================
// Gets the width of the Far plane
//==============================================================================
float G3::FRUSTUM::GetFarPlaneWidth () {
    return AspectRatio * 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * FarPlane;
}

//==============================================================================
// Calculates the corners of the frustum
//==============================================================================
void G3::FRUSTUM::CalculateCorners () {
    // Near plane corners
    float Hnear = 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * NearPlane;
    float Wnear = Hnear * AspectRatio;

    VEC3F NearCenter = Origin + ForwardVector * NearPlane;

    Corner [GF_NEAR_TOPLEFT] = NearCenter + UpVector * Hnear / 2.0f - RightVector * Wnear / 2.0f;
    Corner [GF_NEAR_TOPRIGHT] = NearCenter + UpVector * Hnear / 2.0f + RightVector * Wnear / 2.0f;
    Corner [GF_NEAR_BOTTOMLEFT] = NearCenter - UpVector * Hnear / 2.0f - RightVector * Wnear / 2.0f;
    Corner [GF_NEAR_BOTTOMRIGHT] = NearCenter - UpVector * Hnear / 2.0f + RightVector * Wnear / 2.0f;

    // Far plane corners
    float Hfar = 2.0f * tanf (G3_DEG2RAD (FieldOfView) / 2.0f) * FarPlane;
    float Wfar = Hfar * AspectRatio;

    VEC3F FarCenter = Origin + ForwardVector * FarPlane;

    Corner [GF_FAR_TOPLEFT] = FarCenter + UpVector * Hfar / 2.0f - RightVector * Wfar / 2.0f;
    Corner [GF_FAR_TOPRIGHT] = FarCenter + UpVector * Hfar / 2.0f + RightVector * Wfar / 2.0f;
    Corner [GF_FAR_BOTTOMLEFT] = FarCenter - UpVector * Hfar / 2.0f - RightVector * Wfar / 2.0f;
    Corner [GF_FAR_BOTTOMRIGHT] = FarCenter - UpVector * Hfar / 2.0f + RightVector * Wfar / 2.0f;

    // Recalculate the bounding box
    for (int i=0; i<8; i++) {
        if (i == 0) {
            BBox.Minimum = Corner [i];
            BBox.Maximum = Corner [i];
        } else
            BBox.Expand (Corner [i]);
    }
}

//==============================================================================
// Clears the list of intersections
//==============================================================================
void G3::FRUSTUM::ClearIntersections () {
    for (int i=0; i<8; i++)
        Intersections [i].Flags = 0;
}

//==============================================================================
// Calculates Frustum - Plane intersections
//==============================================================================
void G3::FRUSTUM::IntersectPlane (PLANE3F &Plane) {
    for (int i=0; i<4; i++)
        Intersections [i] = RayPlaneIntersection (Plane, RAY3F (Corner [i], Corner [i + 4]));

    Intersections [GF_NEARLEFT] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_NEAR_TOPLEFT], Corner [GF_NEAR_BOTTOMLEFT]));
    Intersections [GF_NEARRIGHT] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_NEAR_TOPRIGHT], Corner [GF_NEAR_BOTTOMRIGHT]));
    Intersections [GF_NEARTOP] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_NEAR_TOPLEFT], Corner [GF_NEAR_TOPRIGHT]));
    Intersections [GF_NEARBOTTOM] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_NEAR_BOTTOMLEFT], Corner [GF_NEAR_BOTTOMRIGHT]));

    Intersections [GF_FARLEFT] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_FAR_TOPLEFT], Corner [GF_FAR_BOTTOMLEFT]));
    Intersections [GF_FARRIGHT] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_FAR_TOPRIGHT], Corner [GF_FAR_BOTTOMRIGHT]));
    Intersections [GF_FARTOP] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_FAR_TOPLEFT], Corner [GF_FAR_TOPRIGHT]));
    Intersections [GF_FARBOTTOM] = RayPlaneIntersection (Plane, RAY3F (Corner [GF_FAR_BOTTOMLEFT], Corner [GF_FAR_BOTTOMRIGHT]));
}

//==============================================================================
// Calculates the frustum corners and renders its frame
//==============================================================================
void G3::FRUSTUM::glRenderFrame (COL3F Color) {
    bool Lighting = false;
    bool Texture = false;

    glPushMatrix ();
    if (glIsEnabled (GL_TEXTURE_2D)) {
        glDisable (GL_TEXTURE_2D);
        Texture = true;
    }
    if (glIsEnabled (GL_LIGHTING)) {
        glDisable (GL_LIGHTING);
        Lighting = true;
    }

    glEnable (GL_LINE_SMOOTH);
    glLineWidth (2.0f);

    glColor3f (Color.r, Color.g, Color.b);

    glBegin (GL_LINE_STRIP);
    glVertex3f (Corner [GF_FAR_BOTTOMLEFT].x, Corner [GF_FAR_BOTTOMLEFT].y, Corner [GF_FAR_BOTTOMLEFT].z);
    glVertex3f (Corner [GF_FAR_TOPLEFT].x, Corner [GF_FAR_TOPLEFT].y, Corner [GF_FAR_TOPLEFT].z);
    glVertex3f (Corner [GF_FAR_TOPRIGHT].x, Corner [GF_FAR_TOPRIGHT].y, Corner [GF_FAR_TOPRIGHT].z);
    glVertex3f (Corner [GF_FAR_BOTTOMRIGHT].x, Corner [GF_FAR_BOTTOMRIGHT].y, Corner [GF_FAR_BOTTOMRIGHT].z);
    glVertex3f (Corner [GF_FAR_BOTTOMLEFT].x, Corner [GF_FAR_BOTTOMLEFT].y, Corner [GF_FAR_BOTTOMLEFT].z);
    glVertex3f (Corner [GF_NEAR_BOTTOMLEFT].x, Corner [GF_NEAR_BOTTOMLEFT].y, Corner [GF_NEAR_BOTTOMLEFT].z);
    glVertex3f (Corner [GF_NEAR_TOPLEFT].x, Corner [GF_NEAR_TOPLEFT].y, Corner [GF_NEAR_TOPLEFT].z);
    glVertex3f (Corner [GF_NEAR_TOPRIGHT].x, Corner [GF_NEAR_TOPRIGHT].y, Corner [GF_NEAR_TOPRIGHT].z);
    glVertex3f (Corner [GF_NEAR_BOTTOMRIGHT].x, Corner [GF_NEAR_BOTTOMRIGHT].y, Corner [GF_NEAR_BOTTOMRIGHT].z);
    glVertex3f (Corner [GF_NEAR_BOTTOMLEFT].x, Corner [GF_NEAR_BOTTOMLEFT].y, Corner [GF_NEAR_BOTTOMLEFT].z);
    glEnd ();

    glBegin (GL_LINES);
    glVertex3f (Corner [GF_NEAR_TOPLEFT].x, Corner [GF_NEAR_TOPLEFT].y, Corner [GF_NEAR_TOPLEFT].z);
    glVertex3f (Corner [GF_FAR_TOPLEFT].x, Corner [GF_FAR_TOPLEFT].y, Corner [GF_FAR_TOPLEFT].z);
    glVertex3f (Corner [GF_NEAR_TOPRIGHT].x, Corner [GF_NEAR_TOPRIGHT].y, Corner [GF_NEAR_TOPRIGHT].z);
    glVertex3f (Corner [GF_FAR_TOPRIGHT].x, Corner [GF_FAR_TOPRIGHT].y, Corner [GF_FAR_TOPRIGHT].z);
    glVertex3f (Corner [GF_NEAR_BOTTOMRIGHT].x, Corner [GF_NEAR_BOTTOMRIGHT].y, Corner [GF_NEAR_BOTTOMRIGHT].z);
    glVertex3f (Corner [GF_FAR_BOTTOMRIGHT].x, Corner [GF_FAR_BOTTOMRIGHT].y, Corner [GF_FAR_BOTTOMRIGHT].z);
    glEnd ();

    glLineWidth (1.0f);
    glDisable (GL_LINE_SMOOTH);

    if (Texture) {
        glEnable (GL_TEXTURE_2D);
    }
    if (Lighting) {
        glEnable (GL_LIGHTING);
    }
    glPopMatrix ();
}
